#include "stdafx.h"
#include <fgcamera.h>
#include <convert.h>
#include ".\cameraview.h"
#include "string.h"
#include <stdio.h>
#include <math.h>

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

BEGIN_MESSAGE_MAP(CCameraView, CView)
	//{{AFX_MSG_MAP(CCameraView)
	ON_WM_CREATE()
	ON_WM_ERASEBKGND()
	ON_WM_HSCROLL()
	ON_WM_VSCROLL()
	ON_WM_SIZE()
	//}}AFX_MSG_MAP
        ON_WM_LBUTTONDOWN()
        ON_WM_LBUTTONUP()
        ON_WM_MOUSEMOVE()
        ON_WM_RBUTTONUP()
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CCameraView

IMPLEMENT_DYNCREATE(CCameraView, CView)

CCameraView::CCameraView()
{
  m_pBitmapInfo=NULL;
  m_pImage=NULL;

  m_HScrollPos=0;
  m_VScrollPos=0;
  m_HScrollMax=0;
  m_VScrollMax=0;
  m_BkgndVisible=TRUE;
  m_ScrollMode=FALSE;
  m_SelectionEnabled=FALSE;
  m_DrawSelection=FALSE;
  recal = FALSE;
  processImage = FALSE;
  // allocate memory. Without express "new" allocations I find odd things may happen to certain variables
  result = new int[ResultNum];
  pResult = new int[ResultNum];
  friendlyRobot = new OBJECT[FriendlyRobots];
  friendlyDir = new OBJECT[FriendlyRobots];
  friendlyID = new OBJECT[FriendlyRobots*(FriendlyRobots-1)/2];
  enemyRobot = new OBJECT[EnemyRobots];
  filter = new COLORFILTER[FilterNum];
  keyColors = new COLOR[ColorLength];
  linked = new ELEMENT[ImageSize];
  final = new ELEMENT[FinalNum];
  frameCount = 0;
}

CCameraView::~CCameraView()
{
  // Remove all bitmap stuff
  DeleteBitmap();
}


/////////////////////////////////////////////////////////////////////////////
// Diagnose CCameraView

#ifdef _DEBUG
void CCameraView::AssertValid() const
{
	CView::AssertValid();
}

void CCameraView::Dump(CDumpContext& dc) const
{
	CView::Dump(dc);
}
#endif

/////////////////////////////////////////////////////////////////////////////
// Process a frame that comes from our camera.
// Remark: Don't use pFrame->Length, it can be larger then our image!

void CCameraView::ProcessFrame(UINT8 *pData,UINT32,UINT8 Redraw)
{
  UINT32 BytesPerDstPix;

  // Copy frame to our bitmap
  if(m_pBitmapInfo)
  {
    BytesPerDstPix=3;
    switch(m_ColorMode)
    {
      case CM_Y8    : memcpy(m_pImage,pData,(UINT32)m_XSize*(UINT32)m_YSize);
                      BytesPerDstPix=1;
                      break;

      case CM_RAW8  : ConvertRawY8(m_XSize,m_YSize,pData,m_pImage,m_BayerPattern);
                      break;

      case CM_RGB8  : ConvertRGB(m_XSize,m_YSize,pData,m_pImage);
                      break;

      case CM_RGB16 : ConvertRGB16(m_XSize,m_YSize,pData,m_pImage,m_Y16Invert);
                      break;

      case CM_YUV411: Convert411(m_XSize,m_YSize,pData,m_pImage);
                      break;

      case CM_YUV422: Convert422(m_XSize,m_YSize,pData,m_pImage);
                      break;

      case CM_YUV444: Convert444(m_XSize,m_YSize,pData,m_pImage);
                      break;

      case CM_RAW16 : if(m_Y16Invert)
                       ConvertRawY16a(m_XSize,m_YSize,pData,m_pImage,m_BayerPattern);
                      else
                       ConvertRawY16(m_XSize,m_YSize,pData,m_pImage,m_BayerPattern);
                      break;

      case CM_Y16   : if(m_Y16Invert)
                       ConvertY16a(m_XSize,m_YSize,pData,m_pImage);
                      else
                       ConvertY16(m_XSize,m_YSize,pData,m_pImage);
                      break;

      // No other color formats
      default       : return;
    }

    // Redraw our window
    if(m_hWnd)
    {
      // Redraw complete or just invalidate?
      if(Redraw)
       RedrawWindow(NULL,NULL,RDW_INVALIDATE|RDW_UPDATENOW);
      else
       InvalidateRect(NULL,FALSE);  
    }  
  }
}

/////////////////////////////////////////////////////////////////////////////
// Init gray bitmap.

void CCameraView::InitGrayBitmap()
{
  UINT32 i,j,Width,Height;
  UINT8  Value,*pMem;

  Width=abs(m_pBitmapInfo->bmiHeader.biWidth);
  Height=abs(m_pBitmapInfo->bmiHeader.biHeight);
  pMem=m_pImage;

  for(i=0;i<Height;i++)
  {
    Value=(UINT8)(i&0xFF);
    for(j=0;j<Width;j++)
     *pMem++=(UINT8)(Value&0xFF);
  }
}

/////////////////////////////////////////////////////////////////////////////
// Init color bitmap.

void CCameraView::InitColorBitmap()
{
  UINT32  i,j,Width,Height;
  UINT32  Pattern,Offset;
  UINT32  StartPattern;
  UINT8  *pMem;

  Width=abs(m_pBitmapInfo->bmiHeader.biWidth);
  Height=abs(m_pBitmapInfo->bmiHeader.biHeight);

  // Clear whole memory
  memset(m_pImage,0,Width*Height*3);
  pMem=m_pImage;
  StartPattern=0;

  for(i=0;i<Height;i++)
  {
    Pattern=StartPattern;
    Offset=(Pattern>>8)%3;

    for(j=0;j<Width;j++)
    {
      pMem[Offset]=(UINT8)(Pattern&0xFF);
      pMem+=3;

      Pattern+=2;
      Offset=(Pattern>>8)%3;
    }

    StartPattern+=2;
  }
}

/////////////////////////////////////////////////////////////////////////////
// Create a bitmap info for blitting.

BOOL CCameraView::CreateBitmap(UINT32 Width,UINT32 Height,UINT8 ColorMode,
                               UINT8 BayerPattern,UINT8 RawMode,UINT8 Y16Invert)
{
  UINT32 BytesPerPixel,ColorEntries;
  UINT32 i;

  // Width always is a multiple of 4
  Width=(Width+3)&0xFFFFFFFC;
  
  // Always cleanup in advance
  DeleteBitmap();

  // Copy parameters
  m_XSize=m_OrgXSize=(UINT16)Width;
  m_YSize=m_OrgYSize=(UINT16)Height;
  m_ColorMode=ColorMode;
  m_BayerPattern=BayerPattern;
  m_Y16Invert=Y16Invert;
  
  // Replace raw modes
  if((m_ColorMode==CM_Y8) && RawMode)
   m_ColorMode=CM_RAW8;

  if((m_ColorMode==CM_Y16) && RawMode)
   m_ColorMode=CM_RAW16;

  // Get number of bits per pixel
  switch(m_ColorMode)
  {
    case CM_Y8    : BytesPerPixel=1;
                    ColorEntries=256;
                    break;

    case CM_RAW8:
    case CM_RAW16:
    case CM_YUV411:
    case CM_YUV422:
    case CM_YUV444:
    case CM_RGB8  :
    case CM_RGB16 :
    case CM_Y16   : BytesPerPixel=3;
                    ColorEntries=0;
                    break;

    default       : return FALSE;
  }

  m_pImage=(UINT8*)new char[Width*Height*BytesPerPixel];
  m_pBitmapInfo=(BITMAPINFO*)(new char[sizeof(BITMAPINFOHEADER)+
                                       ColorEntries*sizeof(RGBQUAD)]);
  // Return if no memory
  if(!m_pBitmapInfo||!m_pImage)
  {
    DeleteBitmap();
    return FALSE;
  }

  // Set info header
  m_pBitmapInfo->bmiHeader.biSize=sizeof(BITMAPINFOHEADER);
  m_pBitmapInfo->bmiHeader.biWidth=Width;
  m_pBitmapInfo->bmiHeader.biHeight=-(LONG)Height;
  m_pBitmapInfo->bmiHeader.biPlanes=1;
  m_pBitmapInfo->bmiHeader.biBitCount=(UINT16)(BytesPerPixel*8);
  m_pBitmapInfo->bmiHeader.biCompression=BI_RGB;
  m_pBitmapInfo->bmiHeader.biSizeImage=Width*Height*BytesPerPixel;
  m_pBitmapInfo->bmiHeader.biXPelsPerMeter=0;
  m_pBitmapInfo->bmiHeader.biYPelsPerMeter=0;
  m_pBitmapInfo->bmiHeader.biClrUsed=ColorEntries;
  m_pBitmapInfo->bmiHeader.biClrImportant=0;

  // Set color values if greyscale
  if(BytesPerPixel==1)
  {
    // Set our gray scale values
    for(i=0;i<256;i++)
    {
      m_pBitmapInfo->bmiColors[i].rgbRed=(UINT8)i;
      m_pBitmapInfo->bmiColors[i].rgbGreen=(UINT8)i;
      m_pBitmapInfo->bmiColors[i].rgbBlue=(UINT8)i;
      m_pBitmapInfo->bmiColors[i].rgbReserved=0;
    }
    InitGrayBitmap();
  }
  else
  {
    InitColorBitmap();
  }

  if(m_hWnd!=NULL)
  {
    m_SrcRect.left=m_SrcRect.top=0;
    if(m_pBitmapInfo)
    {
      m_SrcRect.right=m_pBitmapInfo->bmiHeader.biWidth;
      m_SrcRect.bottom=abs(m_pBitmapInfo->bmiHeader.biHeight);
    }
    
    RecalcScrollbars();
    InvalidateRect(NULL,TRUE);
  }

  return TRUE;
}

/////////////////////////////////////////////////////////////////////////////
// Delete bitmap info (used for blitting).

void CCameraView::DeleteBitmap()
{
  if(m_pBitmapInfo)
  {
    // Just free our bitmap info
    delete[] m_pBitmapInfo;
    m_pBitmapInfo=NULL;
  }

  if(m_pImage)
  {
    delete[] m_pImage;
    m_pImage=NULL;
  }

  if(m_hWnd!=NULL)
   InvalidateRect(NULL,TRUE);
}

/////////////////////////////////////////////////////////////////////////////
// Reaction on window create.

int CCameraView::OnCreate(LPCREATESTRUCT lpCreateStruct)
{
  if (CView::OnCreate(lpCreateStruct) == -1)
   return -1;

  return 0;
}

/////////////////////////////////////////////////////////////////////////////
// Zeichnung CCameraView

BOOL CCameraView::OnEraseBkgnd(CDC* pDC)
{
  if(!m_pBitmapInfo)
  {
    return CView::OnEraseBkgnd(pDC);
  }
  else
  {
    m_EraseBkgnd=TRUE;
    return TRUE;
  }
}

////////////////////////////////////////////////////////////////////////////////
// Set scroller.

void CCameraView::SetScroller(DWORD Which,int Min,int Max,UINT Page,int Pos,
                              BOOL Redraw)
{
  SCROLLINFO Info;

  Info.cbSize=sizeof(SCROLLINFO);
  Info.fMask=SIF_ALL;
  Info.nMin=Min;
  Info.nMax=Max;
  Info.nPage=Page;
  Info.nPos=Pos;

  SetScrollInfo(Which,&Info,Redraw);
}

////////////////////////////////////////////////////////////////////////////////
// Adjust our blit rectangles (for ScrollMode).
// We assume that the right and bottom coordinate is NOT included.
// XSize and YSize is the size of our client windows area.

void CCameraView::AdjustBlitRectangles(DWORD XSize,DWORD YSize)
{
  DWORD Width,Height;

  if(!m_pBitmapInfo||!m_pImage)
   return;

  Width=(DWORD)abs(m_pBitmapInfo->bmiHeader.biWidth);
  Height=(DWORD)abs(m_pBitmapInfo->bmiHeader.biHeight);

  m_BkgndVisible=FALSE;

  // Adjust X direction
  if(XSize>=Width)
  {
    // Center if all visible
    m_BkgndVisible=TRUE;

    m_SrcRect.left=0;
    m_SrcRect.right=Width;
    m_DstRect.left=(XSize-Width)/2;
    m_DstRect.right=m_DstRect.left+Width;
  }
  else
  {
    m_SrcRect.left=m_HScrollPos;
    m_SrcRect.right=m_SrcRect.left+XSize;
    m_DstRect.left=0;
    m_DstRect.right=XSize;
  }

  // Adjust Y direction
  if(YSize>=Height)
  {
    m_BkgndVisible=TRUE;

    m_SrcRect.top=0;
    m_SrcRect.bottom=Height;
    m_DstRect.top=(YSize-Height)/2;
    m_DstRect.bottom=m_DstRect.top+Height;
  }
  else
  {
    m_SrcRect.top=m_VScrollPos;
    m_SrcRect.bottom=m_SrcRect.top+YSize;
    m_DstRect.top=0;
    m_DstRect.bottom=YSize;
  }
}

/////////////////////////////////////////////////////////////////////////////
// Reaction on horizontal scroller.

void CCameraView::OnHScroll(UINT nSBCode, UINT nPos, CScrollBar*)
{
  int  iScrollPos;
  RECT r;

  iScrollPos=m_HScrollPos;
  switch(nSBCode)
  {
    case SB_LINERIGHT:
     iScrollPos+=1;
     if(iScrollPos>(int)m_HScrollMax)
      iScrollPos=m_HScrollMax;
     break;

    case SB_LINELEFT:
     iScrollPos-=1;
     if(iScrollPos<0)
      iScrollPos=0;
     break;

    case SB_PAGERIGHT:
     iScrollPos+=20*1;
     if(iScrollPos>(int)m_HScrollMax)
      iScrollPos=m_HScrollMax;
     break;

    case SB_PAGELEFT:
     iScrollPos-=20*1;
     if(iScrollPos<0)
      iScrollPos=0;
     break;

    case SB_THUMBTRACK:
     iScrollPos=(short int)nPos;
     break;
  }

  if(iScrollPos!=(int)m_HScrollPos)
  {
    m_HScrollPos=iScrollPos;
    SetScrollPos(SB_HORZ,iScrollPos,TRUE);
    GetClientRect(&r);
    AdjustBlitRectangles(r.right-r.left,r.bottom-r.top);
    InvalidateRect(NULL,TRUE);
  }
}

/////////////////////////////////////////////////////////////////////////////
// Reaction on vertical scroller.

void CCameraView::OnVScroll(UINT nSBCode, UINT nPos, CScrollBar*)
{
  int  iScrollPos;
  RECT r;

  iScrollPos=m_VScrollPos;
  switch(nSBCode)
  {
    case SB_LINERIGHT:
     iScrollPos+=1;
     if(iScrollPos>(int)m_VScrollMax)
      iScrollPos=m_VScrollMax;
     break;

    case SB_LINELEFT:
     iScrollPos-=1;
     if(iScrollPos<0)
      iScrollPos=0;
     break;

    case SB_PAGERIGHT:
     iScrollPos+=20*1;
     if(iScrollPos>(int)m_VScrollMax)
      iScrollPos=m_VScrollMax;
     break;

    case SB_PAGELEFT:
     iScrollPos-=20*1;
     if(iScrollPos<0)
      iScrollPos=0;
     break;

    case SB_THUMBTRACK:
     iScrollPos=(short int)nPos;
     break;
  }

  if(iScrollPos!=(int)m_VScrollPos)
  {
    m_VScrollPos=iScrollPos;
    SetScrollPos(SB_VERT,iScrollPos,TRUE);
    GetClientRect(&r);
    AdjustBlitRectangles(r.right-r.left,r.bottom-r.top);
    InvalidateRect(NULL,TRUE);
  }
}

////////////////////////////////////////////////////////////////////////////////
// Adjust our scrollbars.

void CCameraView::RecalcScrollbars()
{
  int   Dif;
  UINT  SPage;
  UINT  SMax;
  DWORD XSize,YSize,Width,Height;
  RECT  rClient;
  BOOL  HTooSmall;

  if(!m_pBitmapInfo || !m_pImage)
   return;

  if(m_ScrollMode)
  {
    Width=abs(m_pBitmapInfo->bmiHeader.biWidth);
    Height=abs(m_pBitmapInfo->bmiHeader.biHeight);

    // Get pure client area
    GetClientRect(&rClient);
    XSize=rClient.right-rClient.left;
    YSize=rClient.bottom-rClient.top;

    // Add scroller area if we have scrollers
    if(m_VScrollMax!=0)
     XSize+=GetSystemMetrics(SM_CXVSCROLL);
    if(m_HScrollMax!=0)
     YSize+=GetSystemMetrics(SM_CYHSCROLL);

    // Check whether we need scrollers and correct the size
    HTooSmall=FALSE;
    if(Width>XSize)
    {
      YSize-=GetSystemMetrics(SM_CYHSCROLL);
      HTooSmall=TRUE;
    }

    if(Height>YSize)
    {
      XSize-=GetSystemMetrics(SM_CXVSCROLL);
      // Check HSize again
      if(!HTooSmall)
       if(Width>XSize)
        YSize-=GetSystemMetrics(SM_CYHSCROLL);
    }

    // Set horizontal scrollbar
    if((Dif=Width-XSize)>0)
    {
      m_HScrollMax=Dif;

      // Probably reduce scroll position on enlarging
      if(m_HScrollPos+XSize>Width)
       m_HScrollPos=(Width-XSize);

      SPage=((Width-XSize+1)*XSize)/(Width-XSize);
      SMax=m_HScrollMax+SPage-1;
      SetScroller(SB_HORZ,0,SMax,SPage,m_HScrollPos);
    }
    else
    {
      SetScroller(SB_HORZ,0,0,0,0);
      m_HScrollMax=0;
      m_HScrollPos=0;
    }

    // Set vertical scrollbar
    if((Dif=Height-YSize)>0)
    {
      m_VScrollMax=Dif;

      // Probably reduce scroll position on enlarging
      if(m_VScrollPos+YSize>Height)
       m_VScrollPos=(Height-YSize);

      SPage=((Height-YSize+1)*YSize)/(Height-YSize);
      SMax=m_VScrollMax+SPage-1;
      SetScroller(SB_VERT,0,SMax,SPage,m_VScrollPos);
    }
    else
    {
      SetScroller(SB_VERT,0,0,0,0);
      m_VScrollPos=0;
      m_VScrollMax=0;
    }

    AdjustBlitRectangles(XSize,YSize);
  }
  else
  {
    SetScroller(SB_HORZ,0,0,0,0);
    SetScroller(SB_VERT,0,0,0,0);
    GetClientRect(&m_DstRect);
  }
  
}

////////////////////////////////////////////////////////////////////////////////
// Adjust our scrollbars.

void CCameraView::OnSize(UINT nType, int cx, int cy)
{
  RecalcScrollbars();
  CView::OnSize(nType, cx, cy);
}

////////////////////////////////////////////////////////////////////////////////
// Reaction on scrollmode entry.

void CCameraView::SetScrollmode(BOOL Enable)
{
  m_ScrollMode=Enable;

  m_HScrollPos=0;
  m_VScrollPos=0;

  if(!m_ScrollMode)
  {
    m_HScrollMax=0;
    m_VScrollMax=0;
    
    if(m_pBitmapInfo)
    {
      m_SrcRect.left=m_SrcRect.top=0;
      m_SrcRect.right=m_pBitmapInfo->bmiHeader.biWidth;
      m_SrcRect.bottom=abs(m_pBitmapInfo->bmiHeader.biHeight);
    }  
  }

  RecalcScrollbars();
  InvalidateRect(NULL,TRUE);
}

/////////////////////////////////////////////////////////////////////////////
// Erase background.

void CCameraView::EraseBkgnd(CDC *pDC)
{
  CBrush *pOldBrush,*pBrush;
  CPen   *pOldPen,*pPen;
  RECT    Rect;

  if(m_ScrollMode && m_BkgndVisible)
  {
    pBrush=new CBrush;
    pPen=new CPen;

    if(pBrush && pPen)
    {
      pBrush->CreateSolidBrush(RGB(64,64,64));
      pPen->CreatePen(PS_SOLID,1,RGB(64,64,64));

      pOldBrush=pDC->SelectObject(pBrush);
      pOldPen=pDC->SelectObject(pPen);

      GetClientRect(&Rect);

      // Draw 4 rectangles to hide background
      pDC->Rectangle(Rect.left,Rect.top,Rect.right,m_DstRect.top);
      pDC->Rectangle(Rect.left,m_DstRect.top,m_DstRect.left,m_DstRect.bottom);
      pDC->Rectangle(m_DstRect.right,m_DstRect.top,Rect.right,m_DstRect.bottom);
      pDC->Rectangle(Rect.left,m_DstRect.bottom,Rect.right,Rect.bottom);

      pDC->SelectObject(pOldBrush);
      pDC->SelectObject(pOldPen);
    }

    if(pBrush)
     delete pBrush;

    if(pPen)
     delete pPen;
  }

  m_EraseBkgnd=FALSE;
}

void CCameraView::OnDraw(CDC* pDC)
{
  DWORD Height;
  char *pStr;
  char *xStr = new char[30];
  char *yStr = new char[30];

	int i;
	if( recal ){
		for(i=0;i<ColorLength;i++){
			keyColors[i].Blue = -1;
			keyColors[i].Green = -1;
			keyColors[i].Red = -1;
		}
	}

  // Check background erase
  if(m_EraseBkgnd)
   EraseBkgnd(pDC);

  // Check whether we have a bitmap or not
  if(m_pBitmapInfo)
  {
    // Set appropriate blitmode
    pDC->SetStretchBltMode(STRETCH_DELETESCANS);
    Height=abs(m_pBitmapInfo->bmiHeader.biHeight);

    if(!m_ScrollMode)
    {
		if( processImage ){
			ProcessImage();
			
			switch (colorSelectStage)
			{
			case 0:
				xStr = "Select the color of your team";
				yStr = "";
			break;
			case 1:
				xStr = "Select the color of the opposing team";
				yStr = "";
			break;
			case 2:
				xStr = "Select the direction color";
				yStr = "";
			break;
			case 3:
				xStr = "Select the ID# color";
				yStr = "";
			break;
			case 4:
				xStr = "Select the ball color";
				yStr = "";
			break;
			case 5:
				xStr = "Select the field line color";
				yStr = "";
			break;
			default:
				xStr = itoa(result[0], xStr, 10);
				yStr = itoa(result[1], yStr, 10);
			}
		}
		else
		{
			xStr = itoa(mPoint.x, xStr, 10);
			yStr = itoa(mPoint.y, yStr, 10);
		}

      // Blit bitmap (inverted HEIGHT for top down bitmap)
      StretchDIBits(pDC->m_hDC,
                    m_DstRect.left,
                    m_DstRect.top,
                    m_DstRect.right-m_DstRect.left,
                    m_DstRect.bottom-m_DstRect.top,
                    m_SrcRect.left,
                    Height-m_SrcRect.bottom,
                    m_SrcRect.right-m_SrcRect.left,
                    m_SrcRect.bottom-m_SrcRect.top,
                    m_pImage,m_pBitmapInfo,DIB_RGB_COLORS,SRCCOPY);

    }
    else
    {
      // Windows definitely has a blitting bug:
      // It shows if we have a vertical scroller and we blit with vertical scroller
      // set to max and start with source rectangle zero
      if((m_VScrollMax!=0) && (m_VScrollPos==m_VScrollMax) && (m_HScrollPos==0))
      {
        m_pBitmapInfo->bmiHeader.biHeight*=-1;
        StretchDIBits(pDC->m_hDC,
                      m_DstRect.left,
                      m_DstRect.top,
                      m_DstRect.right-m_DstRect.left,
                      m_DstRect.bottom-m_DstRect.top,
                      m_SrcRect.left,
                      m_SrcRect.bottom+1,
                      m_SrcRect.right-m_SrcRect.left,
                      -(m_SrcRect.bottom-m_SrcRect.top),
                      m_pImage,m_pBitmapInfo,DIB_RGB_COLORS,SRCCOPY);
        m_pBitmapInfo->bmiHeader.biHeight*=-1;
      }
      else
      {
        StretchDIBits(pDC->m_hDC,
                      m_DstRect.left,
                      m_DstRect.top,
                      m_DstRect.right-m_DstRect.left,
                      m_DstRect.bottom-m_DstRect.top,
                      m_SrcRect.left,
                      Height-m_SrcRect.bottom,
                      m_SrcRect.right-m_SrcRect.left,
                      m_SrcRect.bottom-m_SrcRect.top,
                      m_pImage,m_pBitmapInfo,DIB_RGB_COLORS,SRCCOPY);
      }
    }
    
    if(m_DrawSelection)
    {
      pDC->SelectStockObject(HOLLOW_BRUSH);
      pDC->SelectStockObject(WHITE_PEN);
      pDC->SetROP2(R2_XORPEN);
      
      pDC->Rectangle(&m_DragRect);
    }
  }
  else
  {
    pStr="Camera has no valid settings!";
    pDC->TextOut(0,0,pStr,(int)strlen(pStr));
  }
  
	if( colorSelectStage > 5)
	{
		xStr = "friendly";
		for(i=0;i<15;i+=3)
		{
			if( result[i] > XStart ){
				pDC->TextOut(result[i]+10,result[i+1]+10,xStr,(int)strlen(xStr));
				yStr = itoa(i/3, yStr, 10);
				pDC->TextOut(result[i]+10,result[i+1]+26,yStr,(int)strlen(yStr));
				yStr = itoa(result[i+2], yStr, 10);
				pDC->TextOut(result[i]+30,result[i+1]+26,yStr,(int)strlen(yStr));
			}
		}
		xStr = "enemy";
		for(i=15;i<30;i+=3)
		{
			if( result[i] > XStart )
				pDC->TextOut(result[i]+10,result[i+1]+10,xStr,(int)strlen(xStr));
		}
		if( result[30] > XStart )
		{
			if(ball.direction == 0)
			{
				xStr = "ball";
				pDC->TextOut(result[30]+10,result[31]+10,xStr,(int)strlen(xStr));
			}
			else if(ball.direction == 1)
			{
				xStr = "ball!";
				pDC->TextOut(result[30]+10,result[31]+10,xStr,(int)strlen(xStr));
			}
			else if(ball.direction == 2)
			{
				xStr = "ball?";
				pDC->TextOut(result[30]+10,result[31]+10,xStr,(int)strlen(xStr));
			}
		}
	}
	else
	{
		pDC->TextOut(0,0,xStr,(int)strlen(xStr));
		pDC->TextOut(0,16,yStr,(int)strlen(yStr));
	}
}

void CCameraView::OnLButtonDown(UINT nFlags, CPoint point)
{
  if(!m_ScrollMode && m_SelectionEnabled)
  {
    SetCapture();
    m_DrawSelection=TRUE;
    m_StartPoint=point;
    m_DragRect.left=m_DragRect.right=point.x;
    m_DragRect.top=m_DragRect.bottom=point.y;
  }
  
  CView::OnLButtonDown(nFlags, point);
}

void CCameraView::OnMouseMove(UINT nFlags, CPoint point)
{
  RECT Rect,Client;
  
  mPoint = point;

  if(m_DrawSelection)
  {
    m_DragRect.left=m_StartPoint.x;
    m_DragRect.top=m_StartPoint.y;
    m_DragRect.right=point.x;
    m_DragRect.bottom=point.y;
    
    Rect=m_DragRect;
    
    if(Rect.top>Rect.bottom)
    {
      m_DragRect.bottom=Rect.top;
      m_DragRect.top=Rect.bottom;
    }
    
    if(Rect.left>Rect.right)
    {
      m_DragRect.right=Rect.left;
      m_DragRect.left=Rect.right;
    }
    
    GetClientRect(&Client);
    
    if(m_DragRect.left<0)
     m_DragRect.left=0;
     
    if(m_DragRect.top<0)
     m_DragRect.top=0;
    
    if(m_DragRect.right>Client.right)
     m_DragRect.right=Client.right; 
     
    if(m_DragRect.bottom>Client.bottom)
     m_DragRect.bottom=Client.bottom; 
     
    InvalidateRect(NULL,TRUE);
  }
  
  CView::OnMouseMove(nFlags, point);
}

void CCameraView::OnLButtonUp(UINT nFlags, CPoint point)
{
  RECT   Client;
  double XScale,YScale;
  int i;
  
  // Only if select mode was active
  if(m_DrawSelection)
  {

	// Here's some filter getting code
	if(colorSelectStage < 6 && processImage){
		// put the color in filter
		i = mPoint.x+mPoint.y*640;
		filter[colorSelectStage].Blue = linked[i].Blue;
		filter[colorSelectStage].Green = linked[i].Green;
		filter[colorSelectStage].Red = linked[i].Red;
		filter[colorSelectStage].leniency = DefaultLeniency;
		colorSelectStage++;
	}
	//********************************

    ReleaseCapture();
    m_DrawSelection=FALSE;
    
    if(m_DragRect.right>=m_DragRect.left+4 && m_DragRect.bottom>=m_DragRect.top)
    {
      GetClientRect(&Client);
     
      XScale=(double)(m_SrcRect.right-m_SrcRect.left)/(double)Client.right;
      YScale=(double)(m_SrcRect.bottom-m_SrcRect.top)/(double)Client.bottom;
    
      // Get real rectangle within actual source rectangle
      m_DragRect.top=(LONG)(m_DragRect.top*YScale+0.5);
      m_DragRect.bottom=(LONG)(m_DragRect.bottom*YScale+0.5);
      if(m_DragRect.top==m_DragRect.bottom)
       m_DragRect.bottom=m_DragRect.top+1;
     
      m_DragRect.left=(LONG)(m_DragRect.left*XScale+0.5);
      m_DragRect.right=(LONG)(m_DragRect.right*XScale+0.5);
      if(m_DragRect.left==m_DragRect.right)
       m_DragRect.right=m_DragRect.left+1;
    
      ::PostMessage(m_hMsgWnd,m_Msg,
                    MAKELONG((USHORT)m_DragRect.left,(USHORT)m_DragRect.right),
                    MAKELONG((USHORT)m_DragRect.top,(USHORT)m_DragRect.bottom));  

      // Redraw to hide rectangle
      InvalidateRect(NULL,TRUE);
    }  
  }
  
  CView::OnLButtonUp(nFlags, point);
}

void CCameraView::EnableSelection(BOOL Enable,HWND hWnd,int Msg)
{
  m_Msg=Msg;
  m_hMsgWnd=hWnd;
  m_SelectionEnabled=Enable;
}

void CCameraView::OnRButtonUp(UINT nFlags, CPoint point)
{
  if(!m_ScrollMode && m_SelectionEnabled)
   ::PostMessage(m_hMsgWnd,m_Msg,0,0);
   
  CView::OnRButtonUp(nFlags, point);
}

void CCameraView::Resize(RECT *pSrcRect,UINT16 NewWidth,UINT16 NewHeight)
{
  BOOL MustClear;
  
  // Not in scroll mode
  if(m_ScrollMode)
   return;

  MustClear=FALSE;
  if(!pSrcRect)
  {
    // Check for clear
    if(m_XSize!=m_OrgXSize || m_YSize!=m_OrgYSize)
     MustClear=TRUE;
    
    // Reset and show full bitmap
    m_SrcRect.left=m_SrcRect.top=0;
    m_SrcRect.right=m_XSize=m_OrgXSize;
    m_SrcRect.bottom=m_YSize=m_OrgYSize;
  }
  else
  {
    m_SrcRect=*pSrcRect;
    if(NewWidth)
    {
      MustClear=TRUE;
      m_XSize=NewWidth;
    }  
    
    if(NewHeight) 
    {
      MustClear=TRUE;
      m_YSize=NewHeight;
    }  
  }  
  
  m_pBitmapInfo->bmiHeader.biWidth=m_XSize;
  m_pBitmapInfo->bmiHeader.biHeight=-m_YSize;

  if(MustClear)
   memset(m_pImage,0,m_pBitmapInfo->bmiHeader.biSizeImage);

  InvalidateRect(NULL,FALSE);
}


int* CCameraView::ProcessImage()
{
  int i,j,k,l,m,n;
  UINT8  *pMem;

  int colorFind[6];
  int colorSet[3];
	
  int minDist, oldMin, minDex, minDex2;
  bool paletteMerge = true;
  
  int nextLabel;
  int minLabel;

  int threshold = 0;
  COLOR cPixel;
  COLOR cTest;
  int maxDist;
  int oldDist;

  bool neighbors = false;

  bool newLabel;
  int pixDist;

  int averageSize = 0;
  OBJECT temp;

  // initialize all results to zero
  for(i=0;i<33;i++)
	  result[i] = 0;

  // here we see if we're recalculating the background color
  // this will be performed one time each time ProcessRobots is turned on
  if(recal)
  {
		recal = FALSE;
		// initialize stuff
		minLabel = 0;

		pMem=m_pImage;

		minDex = 0;
		minDex2 = 0;

		maxDist = colorDistance(0,0,0,255,255,255);

		pMem += (YStart*640+XStart)*3;
		for(i=YStart;i<YEnd;i++)
		{	
			for(j=XStart;j<XEnd;j++)
			{
				cPixel.Blue = pMem[0];
				cPixel.Green = pMem[1];
				cPixel.Red = pMem[2];
				cPixel.times = 1;

				oldMin = maxDist;

				for(k=0;k<ColorLength;k++)
				{
					if( keyColors[k].Blue == -1 ){
						minDex = -1;
						keyColors[k] = cPixel;
						break;
					}
					else
					{
						minDist = colorDistance(cPixel.Blue, cPixel.Green, cPixel.Red, keyColors[k].Blue,
							keyColors[k].Green, keyColors[k].Red);
						if( minDist < oldMin){
							minDex = k;
							oldMin = minDist;
							paletteMerge = false;
						}
						for(l=k+1;l<ColorLength;l++)
						{
							minDist = colorDistance(keyColors[k].Blue, keyColors[k].Green, keyColors[k].Red, keyColors[l].Blue,
								keyColors[l].Green, keyColors[l].Red);
							if( minDist < oldMin){
								minDex = k;
								minDex2 = l;
								oldMin = minDist;
								paletteMerge = true;
							}
						}
					}
				}
				if( paletteMerge && minDex >= 0 )
				{
					keyColors[minDex].Blue = (keyColors[minDex].Blue*keyColors[minDex].times 
												+ keyColors[minDex2].Blue*keyColors[minDex2].times)
												/ (keyColors[minDex].times + keyColors[minDex2].times);
					keyColors[minDex].Green = (keyColors[minDex].Green*keyColors[minDex].times 
												+ keyColors[minDex2].Green*keyColors[minDex2].times)
												/ (keyColors[minDex].times + keyColors[minDex2].times);
					keyColors[minDex].Red = (keyColors[minDex].Red*keyColors[minDex].times 
												+ keyColors[minDex2].Red*keyColors[minDex2].times)
												/ (keyColors[minDex].times + keyColors[minDex2].times);
					keyColors[minDex].times += keyColors[minDex2].times;
					keyColors[minDex2] = cPixel;
				}
				else if( minDex >= 0 )
				{
					keyColors[minDex].Blue = (keyColors[minDex].Blue*keyColors[minDex].times + cPixel.Blue)
											/ (keyColors[minDex].times + 1);
					keyColors[minDex].Green = (keyColors[minDex].Green*keyColors[minDex].times + cPixel.Green)
											/ (keyColors[minDex].times + 1);
					keyColors[minDex].Red = (keyColors[minDex].Red*keyColors[minDex].times + cPixel.Red)
											/ (keyColors[minDex].times + 1);
					keyColors[minDex].times++;
				}

				pMem+=3;		
			}
			pMem+= 3*(640-(XEnd-XStart));
		}

		// average all into first bin
		for(k=1;k<ColorLength;k++)
		{
			keyColors[0].Blue = (keyColors[0].Blue*keyColors[0].times 
								+ keyColors[k].Blue*keyColors[k].times)
								/ (keyColors[0].times + keyColors[k].times);
			keyColors[0].Green = (keyColors[0].Green*keyColors[0].times 
								+ keyColors[k].Green*keyColors[k].times)
								/ (keyColors[0].times + keyColors[k].times);
			keyColors[0].Red = (keyColors[0].Red*keyColors[0].times 
								+ keyColors[k].Red*keyColors[k].times)
								/ (keyColors[0].times + keyColors[k].times);
			keyColors[0].times += keyColors[k].times;
		}

		// make the second bin anything that isn't the green (maybe halfway between the green and white?)
		keyColors[1].Blue = (255 + keyColors[0].Blue)/2;
		keyColors[1].Green = (255 + keyColors[0].Green)/2;
		keyColors[1].Red = (255 + keyColors[0].Red)/2;

		// now for every possible pixel we set the boolean value to foreground or not
		//  we only do this once!
		for(i=0;i<255;i++)
			for(j=0;j<255;j++)
				for(k=0;k<255;k++)
					if(Threshold > abs(i - keyColors[0].Blue) + abs(j - keyColors[0].Green) + abs(k - keyColors[0].Red))
						fore[i][j][k] = false;
					else
						fore[i][j][k] = true;

		// Here I set pResult so we don't draw errors for not having previous result uninitialized
		for(i=0;i<ResultNum;i++)
		{
			pResult[i] = 0;
		}
  }

  // ******************************************************************************
  // Now for the operations we always do every loop
  // ******************************************************************************
  pMem=m_pImage;
  
  for(i=0;i<480;i++)
  {
    for(j=0;j<640;j++)
    {
		// Linked data initialization
		linked[i*640+j].label = 0;
		linked[i*640+j].size = 1;
		linked[i*640+j].parent = 0;
		linked[i*640+j].Blue = pMem[0];
		linked[i*640+j].Green = pMem[1];
		linked[i*640+j].Red = pMem[2];
		linked[i*640+j].y = i;
		linked[i*640+j].x = j;
		pMem+=3;
	}
  }

  pMem=m_pImage;
  nextLabel = 1;

  pMem += (YStart*640+XStart)*3;
  for(i=YStart;i<YEnd;i++)
  {
    for(j=XStart;j<XEnd;j++)
    {
		neighbors = false;
		
		/*
		// inline doesn't seem to be working correctly...
		minDex = abs(pMem[0] - keyColors[0].Blue) + abs(pMem[1] - keyColors[0].Green) + abs(pMem[2] - keyColors[0].Red);
		//minDex2 = abs(pMem[0] - keyColors[1].Blue) + abs(pMem[0] - keyColors[1].Green) + abs(pMem[2] - keyColors[1].Red);
		minDex2 = Threshold; // arbitrary threshold for distance.
		//minDex = colorDistance(pMem[0], pMem[1], pMem[2], keyColors[0].Blue,
		//		keyColors[0].Green, keyColors[0].Red);
		//minDex2 = colorDistance(pMem[0], pMem[1], pMem[2], keyColors[1].Blue,
		//		keyColors[1].Green, keyColors[1].Red);
		if( minDex < minDex2 ) // if closer to the background color than the threshold, it's background
			minDex = 0;
		else				// else it must be foreground
			minDex = 1;

		*/

		if( fore[pMem[0]][pMem[1]][pMem[2]] == true ) // We've found something that isn't background.
		{
			// linked[i*640+j] is the current pixel we're looking at
			// Union any neighbors
			if( i > 0 && linked[(i-1)*640+j].label > 0) // if we're not on top row and it isn't background...
			{
				// We found that the pixel above has a label already
				neighbors = true;
				linked[i*640+j].label = linked[(i-1)*640+j].label; // Set currect pixel's label to the one above.
				Union(&linked[i*640+j],&linked[(i-1)*640+j]); // Union the two to the same set
			}
			// Now we're checking the pixel to the left of the current pixel.
			if( j > 0 && linked[i*640+j-1].label > 0)
			{
				// There is one that isn't background.
				if(!neighbors){ // Now see if we didn't find a non-background pixel above.
					// so if not do the same things we did above.
					neighbors = true;
					linked[i*640+j].label = linked[i*640+j-1].label; // same label
					Union(&linked[i*640+j],&linked[i*640+j-1]); // same set
				}
				else // now what should we do if there is a non-background pixel above and to the left?
				{
					// Remember that the current pixel is in the same set and has same label as one above it.

					// So first we see which label is older (lower), now checking if top is better.
					if( linked[i*640+j].label < linked[i*640+j-1].label )
					{
						// here we see that the label from above is better than the one to the left,
						// so we can set the left pixel to our label and set
						linked[i*640+j-1].label = linked[i*640+j].label;
						Union(&linked[i*640+j-1],&linked[i*640+j-1]);
					}
					// or if the one to the left is better (lower = older = better to match with)
					else if( linked[i*640+j-1].label < linked[i*640+j].label )
					{
						// so we set the current pixel to the same label and set as the one to the left...
						linked[i*640+j].label = linked[i*640+j-1].label;
						Union(&linked[i*640+j],&linked[i*640+j-1]);
						// now fix one above to the label and set of the left
						linked[(i-1)*640+j].label = linked[i*640+j-1].label;
						Union(&linked[(i-1)*640+j],&linked[i*640+j-1]);
					}
					// they are the same label already! just unify the above and left to be sure
					else
					{
						Union(&linked[i*640+j-1],&linked[(i-1)*640+j]);
					}
				}
			}
			
			// if he was all alone...
			if(!neighbors)
			{
				linked[i*640+j].label = nextLabel;
				nextLabel++;
			}
		

		}

		pMem+=3;
	}
	pMem+= 3*(640-(XEnd-XStart));
  }

  pMem=m_pImage;
  nextLabel = 0; // for testing purposes here, to see how many valid labels are used
  for(i=0;i<100;i++)
  {
	  final[i].label = NULL;
  }

  pMem += (YStart*640+XStart)*3;
  for(i=YStart;i<YEnd;i++)
  {
	for(j=XStart;j<XEnd;j++)
	{
		
		// if it's a background pixel we paint it black, but paint it pink if it's under the mouse
		if(linked[i*640+j].label == 0)
		{
			if( mPoint.x - 4 < j && mPoint.x + 4 > j && mPoint.y + 4 > i && mPoint.y - 4 < i)
			{
				pMem[0] = 255;
				pMem[1] = 0;
				pMem[2] = 255;
			}
			else{
				pMem[0] = 0;
				pMem[1] = 0;
				pMem[2] = 0;
		
			}
		}
		else
		{
			linked[i*640+j] = *Find(&linked[i*640+j]);
			
			
			if( linked[i*640+j].size > 40 ) // I'm cutting out noise by not caring about small blobs.
			{
				// draw the whole blob with the averaged color
				pMem[0] = linked[i*640+j].Blue;
				pMem[1] = linked[i*640+j].Green;
				pMem[2] = linked[i*640+j].Red;
				
				// add to our list of final labeled blobs if it is a new one
				k=0;
				newLabel = true;
				if( linked[i*640+j].label < 0 || linked[i*640+j].label > 1000) // toss ridiculous labels
					newLabel = false;
				while(final[k].label!=NULL && newLabel){
					if(final[k].label == linked[i*640+j].label){
						newLabel = false;			
					}
					k++;
				}
				if(newLabel){
					final[k] = linked[i*640+j];
					final[k].used = false; // initialize this
					if( averageSize == 0 )
						averageSize = final[k].size;
					else
						averageSize = (averageSize*nextLabel + final[k].size) / (nextLabel+1); 
					nextLabel++; // how many total valid labels.
				}
			}
			else
			{
					pMem[0] = 0;
					pMem[1] = 0;
					pMem[2] = 0;
			}
		}

		pMem+=3;
	}
	pMem+= 3*(640-(XEnd-XStart));
  }

  // **************************************************
  // Here are the final calculations on the found blobs
  // **************************************************
  
  //initialize objects
  ball.x = ball.y = ball.direction = 0;
  for(i=0;i<FriendlyRobots;i++){
	  friendlyRobot[i].x = -1;
	  friendlyRobot[i].y = -1;
	  friendlyRobot[i].direction = -1;
	  friendlyDir[i].x = -1;
	  friendlyDir[i].y = -1;
	  friendlyDir[i].direction = -1;
  }
  for(i=0;i<EnemyRobots;i++){
	  enemyRobot[i].x = -1;
	  enemyRobot[i].y = -1;
	  enemyRobot[i].direction = -1;
  }

  //center colors and field lines
  pMem=m_pImage;
  for(k=0;final[k].label!=NULL;k++){
	// take the field lines out of the calculation by not using huge blobs
	if(final[k].size > averageSize*2)
	{
		final[k].used = true;
	}
	else // for normal blobs
	{
		// color the center of the blobs a special color
		i = ((int)final[k].x+(int)final[k].y*640)*3;
		pMem[i] = 255;
		pMem[i+1] = 255;
		pMem[i+2] = 0;
	}
  }

  j = 0; // will simply be used a counter to track where we put the enemy bots in the array
  l = 0; // used as a counter for the direction blobs
  m = 0; // a counter for friendly robot blobs
  n = 0; // a counter for friendly ID# markers

  if(colorSelectStage > 5){
  for(k=0;final[k].label!=NULL;k++){
	
	final[k].closestDist = DefaultLeniency;

    if(final[k].used == false)
	{
	for(i=0;i<FilterNum;i++){
		// Determine which filter matches the blob
		if(filter[i].Blue > 0 && filter[i].Blue < 256) // filter is really set
		{
			// check if the color of the object falls within the leniency bounds for a filter
			minDist = UVcolorDistance(final[k].Blue,final[k].Green,final[k].Red,
				filter[i].Blue,filter[i].Green,filter[i].Red); 
			if(minDist < filter[i].leniency && minDist < final[k].closestDist)
			{
				final[k].closestDist = minDist;
				final[k].closestMatch = i;
			}
		}
	}

	// now do the appropriate action with the best matching filter for this blob
	switch(final[k].closestMatch){
		case 0:	// friendly filter
			friendlyRobot[m].x = final[k].x;
			friendlyRobot[m].y = final[k].y;
			friendlyRobot[m].filterNum = k;
			if( m < FriendlyRobots )
				m++;
			break;
		case 1: // enemy filter
			enemyRobot[j].x = final[k].x;
			enemyRobot[j].y = final[k].y;
			enemyRobot[j].filterNum = k;
			final[k].used = true;
			if( j < EnemyRobots )
				j++;
			break;
		case 2: // direction marker filter
			friendlyDir[l].x = final[k].x;
			friendlyDir[l].y = final[k].y;
			friendlyDir[l].filterNum = k;
			if( l < FriendlyRobots )
				l++;
			break;
		case 3:	// ID# filter
			friendlyID[n].x = final[k].x;
			friendlyID[n].y = final[k].y;
			friendlyID[n].filterNum = k;
			if( n < FriendlyRobots*(FriendlyRobots-1)/2); // # of markers is addition of sequence
				n++;										// 0+1+2+3+4 = 10 for 5 bots
			break;
		case 4:	// ball filter
			ball.x = final[k].x;
			ball.y = final[k].y;
			ball.filterNum = k;
			final[k].used = true;
			break;
		case 5:	// field line filter
			break;
	}
	}
  }

  // find the direction of robots on the field
  // Loop through each robot and then each direction marker to find the closest one.
  for(i=0;i<FriendlyRobots;i++)
  {
	  oldDist = RoboRadius;
	  minDex = 0;
	  for(j=0;j<FriendlyRobots;j++)
	  {
	    minDist = dist(&friendlyRobot[i],&friendlyDir[j]);
		if(friendlyRobot[i].x != 0 && friendlyDir[j].x != 0
			&& minDist < oldDist)
		{
			oldDist = minDist;
			minDex = j;
		}
	  }
	  // with the direction marker which is closest, set the direction and also record the direction value
	  friendlyRobot[i].direction = angle(&friendlyRobot[i],&friendlyDir[minDex]);
	  // tell the final blob that it was used correctly
	  //final[friendlyDir[minDex].filterNum].used = true;
  }

  // find the ID of our robots
  // This is determined by the number of ID markers within the robot's radius.
  for(i=0;i<FriendlyRobots;i++)
  {
	  // initialize ID's to zero
	  friendlyRobot[i].ID = 0;
	  for(j=0;j<FriendlyRobots*(FriendlyRobots-1)/2;j++)
	  {
	    minDist = dist(&friendlyRobot[i],&friendlyID[j]);
		if(friendlyRobot[i].x != 0 && friendlyID[j].x != 0
			&& minDist < RoboRadius)
		{
			// if the ID marker is within the robot's radius, add one to it's ID value
			friendlyRobot[i].ID++;
			// tell the final blob that it was used correctly
			final[friendlyID[j].filterNum].used = true;
		}
	  }

	  // if this friendlyRobot has found a direction and ID number, we can say its blob was used correctly
	  if(friendlyRobot[i].direction != -1 && friendlyRobot[i].ID != -1 && friendlyRobot[i].filterNum < FinalNum
		  && friendlyRobot[i].filterNum >= 0)
		final[friendlyRobot[i].filterNum].used = true;
  }
  // Sort the robots in our friendly robot array so that their index matches their ID number
  for(i=0;i<FriendlyRobots;i++)
  {
	  if(friendlyRobot[i].ID != i && friendlyRobot[i].ID < FriendlyRobots)
	  {
		  temp = friendlyRobot[friendlyRobot[i].ID];
		  friendlyRobot[friendlyRobot[i].ID] = friendlyRobot[i];
		  friendlyRobot[i] = temp;
	  }
  }

  // we can eliminate all blobs within the radius of enemy robots from further calculation
  for(i=0;i<EnemyRobots;i++)
  {
	  for(k=0;final[k].label!=NULL;k++)
	  {
		  if(dist(&enemyRobot[i],&final[k]) < RoboRadius)
			  final[k].used = true;
	  }
  }

  // Now is the time to check if some things haven't been found, and if not we want
  //  to estimate their positions based on past positions and blobs that are near those.
  
  // for now I'm only checking ball
  if(ball.x == 0){
	  oldDist = SearchDist;
	  ball.direction = 1; // denotes we're just using last position
	  ball.x = pResult[30];
	  ball.y = pResult[31];
	  minDex = -1;
	  for(k=0;final[k].label!=NULL;k++)
	  {
		  if(final[k].used == false)
		  {
			  minDist = dist(&ball,&final[k]);
			  if(minDist < oldDist)
			  {
				  oldDist = minDist;
				  minDex = k;
			  }
		  }
	  }
	  if(minDex > -1)
	  {
		  ball.x = final[minDex].x;
		  ball.y = final[minDex].y;
		  ball.direction = 2; // denotes we're guessing the position
		  final[minDex].used = true;
	  }
  }

  // The result array stores the results and is what is transferred to the ai.
  for(i=0;i<5;i++){
	  result[i*3] = friendlyRobot[i].x;
	  result[i*3+1] = friendlyRobot[i].y;
	  result[i*3+2] = friendlyRobot[i].direction;
  }
  for(i=5;i<10;i++){
	  result[i*3] = enemyRobot[i-5].x;
	  result[i*3+1] = enemyRobot[i-5].y;
	  result[i*3+2] = enemyRobot[i-5].direction;
  }
  result[30] = ball.x;
  result[31] = ball.y;
  result[32] = ball.direction;

  frameCount++;
  comFile = fopen("comFile.txt", "w");

  if( comFile != NULL)
  {
	  fprintf(comFile, "%d\n", frameCount);
	  for(i=0;i<ResultNum;i+=3)
	  {
		  fprintf(comFile, "%d %d %d\n", result[i], result[i+1], result[i+2]);
	  }
	  fclose(comFile);
  }
  else
		result[0] = 0;

  // set the previous result to this one for next time
  for(i=0;i<ResultNum;i++)
	  pResult[i] = result[i];

  }// close bracket on check for all colors selected

  return result;
}


inline int CCameraView::colorDistance(int b1, int g1, int r1, int b2, int g2, int r2)
{
	//return sqrt(pow((float)(b1 - b2),2) + pow((float)(g1 - g2), 2) + pow((float)(r1 - r2), 2));
	return abs(b1-b2)+abs(g1-g2)+abs(r1-r2);
}

inline int CCameraView::UVcolorDistance(int b1, int g1, int r1, int b2, int g2, int r2)
{
	int u1 = min(abs(r1 * -1214 + g1 * -2384 + b1 * 3598 + 4096 + 1048576) >> 13, 240);
	int u2 = min(abs(r2 * -1214 + g2 * -2384 + b2 * 3598 + 4096 + 1048576) >> 13, 240);
	int v1 = min(abs(r1 * 3598 + g1 * -3013 + b1 * -585 + 4096 + 1048576) >> 13, 240);
	int v2 = min(abs(r2 * 3598 + g2 * -3013 + b2 * -585 + 4096 + 1048576) >> 13, 240);
	return sqrt(pow((float)(u1-u2),2) + pow((float)(v1-v2),2));
}

// Distances between objects and between objects and elements
inline int CCameraView::dist(OBJECT *a, OBJECT *b)
{
	return sqrt(pow((float)(a->x - b->x),2) + pow((float)(a->y - b->y), 2));
}

inline int CCameraView::dist(OBJECT *a, ELEMENT *b)
{
	return sqrt(pow((float)(a->x - b->x),2) + pow((float)(a->y - b->y), 2));
}

// For finding the angle between two objects.
inline int CCameraView::angle(OBJECT *a, OBJECT *b)
{
	return atan2((float)(a->y-b->y),(float)(a->x-b->x)) * -180 / PI + 180;
}

/* Color Defining Algorithm Idea:
Setup:
COLORRANGE array of size n, each index holds BGR values for a color
int THRESHOLD is the "closeness" a color must be at all BGR values to be automatically considered the same

Execution:
as we iterate through each pixel's color:
1. if color is "close" to another in COLOR array, using THRESHOLD, then automatically
	average its value with that COLOR.
2. if color is not "close" to another average the two closest COLORs in array, and place
	new color as a COLOR in vacated spot.

Final:
COLOR array should have values to map each color to


This turned out to be just like k-means clustering in implementation. i guess i idependantly invented it?

*/

inline ELEMENT* CCameraView::Find(ELEMENT *x)
{
	if(x->parent == 0 || x->parent == x)
		return x;
	else{
		x->parent = Find(x->parent);
		return x->parent;
	}
}

inline void CCameraView::Union(ELEMENT *x, ELEMENT *y)
{
	ELEMENT *xRoot = Find(x);
	ELEMENT *yRoot = Find(y);
	if(xRoot != yRoot && xRoot->size >= yRoot->size){ // xRoot has a bigger tree
		yRoot->parent = xRoot;
		xRoot->Blue = (xRoot->Blue*xRoot->size + yRoot->Blue*yRoot->size) / (xRoot->size+yRoot->size);
		xRoot->Green = (xRoot->Green*xRoot->size + yRoot->Green*yRoot->size) / (xRoot->size+yRoot->size);
		xRoot->Red = (xRoot->Red*xRoot->size + yRoot->Red*yRoot->size) / (xRoot->size+yRoot->size);
		xRoot->x = (xRoot->x*xRoot->size + yRoot->x*yRoot->size) / (xRoot->size+yRoot->size);
		xRoot->y = (xRoot->y*xRoot->size + yRoot->y*yRoot->size) / (xRoot->size+yRoot->size);
		xRoot->size += yRoot->size;
	}
	else if(xRoot != yRoot && xRoot->size < yRoot->size){ // yRoot has a bigger tree
		xRoot->parent = yRoot;
		yRoot->Blue = (xRoot->Blue*xRoot->size + yRoot->Blue*yRoot->size) / (xRoot->size+yRoot->size);
		yRoot->Green = (xRoot->Green*xRoot->size + yRoot->Green*yRoot->size) / (xRoot->size+yRoot->size);
		yRoot->Red = (xRoot->Red*xRoot->size + yRoot->Red*yRoot->size) / (xRoot->size+yRoot->size);
		yRoot->x = (xRoot->x*xRoot->size + yRoot->x*yRoot->size) / (xRoot->size+yRoot->size);
		yRoot->y = (xRoot->y*xRoot->size + yRoot->y*yRoot->size) / (xRoot->size+yRoot->size);
		yRoot->size += xRoot->size;
	}
}
